package com.crunch.math;

import java.util.Arrays;

/**
 * A class for 4x4 matrices.
 */
public class Matrix44f {
	float[] d;

	/**
	 * Constructs an identity matrix.
	 */
	public Matrix44f() {
		d = new float[] { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f };
	}

	/**
	 * Sets this matrix to the identity matrix.
	 */
	public void setIdentity() {
		d[ 0] = 1.0f;
		d[ 1] = 0.0f;
		d[ 2] = 0.0f;
		d[ 3] = 0.0f;
		d[ 4] = 0.0f;
		d[ 5] = 1.0f;
		d[ 6] = 0.0f;
		d[ 7] = 0.0f;
		d[ 8] = 0.0f;
		d[ 9] = 0.0f;
		d[10] = 1.0f;
		d[11] = 0.0f;
		d[12] = 0.0f;
		d[13] = 0.0f;
		d[14] = 0.0f;
		d[15] = 1.0f;
	}

	/**
	 * Constructs a matrix by copying the one provided.
	 */
	public Matrix44f(Matrix44f m) {
		d = Arrays.copyOf(m.d, m.d.length);
	}

	/**
	 * Sets this matrix by copying the one provided.
	 */
	public void set(Matrix44f m) {
		System.arraycopy(m.d, 0, d, 0, d.length);
	}

	/**
	 * Constructs a matrix from the given elements, specified in row-major order.
	 */
	public Matrix44f(float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13, float m20, float m21, float m22, float m23, float m30, float m31, float m32, float m33) {
		d = new float[] { m00, m01, m02, m03, m10, m11, m12, m13, m20, m21, m22, m23, m30, m31, m32, m33 };
	}

	/**
	 * Sets this matrix from the given elements, specified in row-major order.
	 */
	public void set(float m00, float m01, float m02, float m03, float m10, float m11, float m12, float m13, float m20, float m21, float m22, float m23, float m30, float m31, float m32, float m33) {
		d[ 0] = m00;
		d[ 1] = m01;
		d[ 2] = m02;
		d[ 3] = m03;
		d[ 4] = m10;
		d[ 5] = m11;
		d[ 6] = m12;
		d[ 7] = m13;
		d[ 8] = m20;
		d[ 9] = m21;
		d[10] = m22;
		d[11] = m23;
		d[12] = m30;
		d[13] = m31;
		d[14] = m32;
		d[15] = m33;
	}

	/**
	 * Returns the given element using row-major ordering.
	 */
	public float get(int i) {
		return d[i];
	}

	/**
	 * Sets the given element using row-major ordering.
	 */
	public void set(int i, float v) {
		d[i] = v;
	}

	/**
	 * Returns the element at the given row and column.
	 */
	public float get(int row, int column) {
		return d[row*4 + column];
	}

	/**
	 * Sets the element at the given row and column.
	 */
	public void set(int row, int column, float v) {
		d[row*4 + column] = v;
	}

	/**
	 * Returns the sum of this matrix and {@code rhs}.
	 */
	public Matrix44f add(Matrix44f rhs) {
		return new Matrix44f(this).addAssign(rhs);
	}

	/**
	 * Adds {@code rhs} to this matrix.
	 */
	public Matrix44f addAssign(Matrix44f rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] += rhs.d[i];
		}
		return this;
	}

	/**
	 * Returns the difference between this matrix and {@code rhs}.
	 */
	public Matrix44f subtract(Matrix44f rhs) {
		return new Matrix44f(this).subtractAssign(rhs);
	}

	/**
	 * Subtracts {@code rhs} from this matrix.
	 */
	public Matrix44f subtractAssign(Matrix44f rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] -= rhs.d[i];
		}
		return this;
	}

	/**
	 * Returns this matrix negated.
	 */
	public Matrix44f negate() {
		return new Matrix44f(this).negateAssign();
	}

	/**
	 * Negates this matrix.
	 */
	public Matrix44f negateAssign() {
		for (int i = 0; i < d.length; ++i) {
			d[i] = -d[i];
		}
		return this;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Vector4f multiply(Vector4f rhs) {
		Vector4f vec = new Vector4f();
		for (int y = 0; y < 4; ++y) {
			float v = 0.0f;
			for (int x = 0; x < 4; ++x) {
				v += get(y, x) * rhs.get(x);
			}
			vec.set(y, v);
		}
		return vec;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Matrix44f multiply(Matrix44f rhs) {
		return new Matrix44f(this).multiplyAssign(rhs);
	}

	/**
	 * Multiplies this matrix by {@code rhs}.
	 */
	public Matrix44f multiplyAssign(Matrix44f rhs) {
		float[] dt = new float[d.length];
		for (int y = 0; y < 4; ++y) {
			for (int x = 0; x < 4; ++x) {
				int idx = y*4 + x;
				for (int i = 0; i < 4; ++i) {
					dt[idx] += get(y, i) * rhs.get(i, x);
				}
			}
		}
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Pre-multiplies this matrix by {@code lhs}.
	 */
	public Matrix44f preMultiplyAssign(Matrix44f lhs) {
		float[] dt = new float[d.length];
		for (int y = 0; y < 4; ++y) {
			for (int x = 0; x < 4; ++x) {
				int idx = y*4 + x;
				for (int i = 0; i < 4; ++i) {
					dt[idx] += lhs.get(y, i) * get(i, x);
				}
			}
		}
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Returns the product of this matrix and {@code rhs}.
	 */
	public Matrix44f multiply(float rhs) {
		return new Matrix44f(this).multiplyAssign(rhs);
	}

	/**
	 * Multiplies this matrix by {@code rhs}.
	 */
	public Matrix44f multiplyAssign(float rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] *= rhs;
		}
		return this;
	}

	/**
	 * Returns the quotient of this matrix and {@code rhs}.
	 */
	public Matrix44f divide(float rhs) {
		return new Matrix44f(this).divideAssign(rhs);
	}

	/**
	 * Divides this matrix by {@code rhs}.
	 */
	public Matrix44f divideAssign(float rhs) {
		for (int i = 0; i < d.length; ++i) {
			d[i] /= rhs;
		}
		return this;
	}

	/**
	 * Returns whether this matrix is equal to the object {@code o}.
	 */
	@Override
	public boolean equals(Object o) {
		if (o == this) {
			return true;
		} else if (o == null || !(o instanceof Matrix44f)) {
			return false;
		} else {
			return equals((Matrix44f) o);
		}
	}

	/**
	 * Returns a hash code for this object.
	 */
	@Override
	public int hashCode() {
		int bits = 0;
		for (int i = 0; i < d.length; ++i) {
			bits ^= Float.floatToIntBits(d[i]);
		}
		return bits;
	}

	/**
	 * Returns whether this matrix is equal to the matrix {@code rhs}.
	 */
	public boolean equals(Matrix44f rhs) {
		return Arrays.equals(d, rhs.d);
	}

	/**
	 * Returns the transpose of this matrix.
	 */
	public Matrix44f transpose() {
		return new Matrix44f(this).transposeAssign();
	}

	/**
	 * Transposes this matrix.
	 */
	public Matrix44f transposeAssign() {
		float tmp = d[1];
		d[1] = d[2];
		d[2] = tmp;
		return this;
	}

	/**
	 * Returns the inverse of this matrix, or {@code null} if this matrix is not invertible.
	 */
	public Matrix44f invert() {
		return new Matrix44f(this).invertAssign();
	}

	/**
	 * Inverts this matrix, or returns {@code null} if this matrix is not invertible.
	 */
	public Matrix44f invertAssign() {
		float det = determinant();
		if (det == 0.0f) {
			return null;
		}

		float invDet = 1.0f / det;
		float[] dt = new float[] {
				(d[ 5]*d[10]*d[15] + d[ 6]*d[11]*d[13] + d[ 7]*d[ 9]*d[14] - d[ 5]*d[11]*d[14] - d[ 6]*d[ 9]*d[15] - d[ 7]*d[10]*d[13]) * invDet,
				(d[ 1]*d[11]*d[14] + d[ 2]*d[ 9]*d[15] + d[ 3]*d[10]*d[13] - d[ 1]*d[10]*d[15] - d[ 2]*d[11]*d[13] - d[ 3]*d[ 9]*d[14]) * invDet,
				(d[ 1]*d[ 6]*d[15] + d[ 2]*d[ 7]*d[13] + d[ 3]*d[ 5]*d[14] - d[ 1]*d[ 7]*d[14] - d[ 2]*d[ 5]*d[15] - d[ 3]*d[ 6]*d[13]) * invDet,
				(d[ 1]*d[ 7]*d[10] + d[ 2]*d[ 5]*d[11] + d[ 3]*d[ 6]*d[ 9] - d[ 1]*d[ 6]*d[11] - d[ 2]*d[ 7]*d[ 9] - d[ 3]*d[ 5]*d[10]) * invDet,
				(d[ 4]*d[11]*d[14] + d[ 6]*d[ 8]*d[15] + d[ 7]*d[10]*d[12] - d[ 4]*d[10]*d[15] - d[ 6]*d[11]*d[12] - d[ 7]*d[ 8]*d[14]) * invDet,
				(d[ 0]*d[10]*d[15] + d[ 2]*d[11]*d[12] + d[ 3]*d[ 8]*d[14] - d[ 0]*d[11]*d[14] - d[ 2]*d[ 8]*d[15] - d[ 3]*d[10]*d[12]) * invDet,
				(d[ 0]*d[ 7]*d[14] + d[ 2]*d[ 4]*d[15] + d[ 3]*d[ 6]*d[12] - d[ 0]*d[ 6]*d[15] - d[ 2]*d[ 7]*d[12] - d[ 3]*d[ 4]*d[14]) * invDet,
				(d[ 0]*d[ 6]*d[11] + d[ 2]*d[ 7]*d[ 8] + d[ 3]*d[ 4]*d[10] - d[ 0]*d[ 7]*d[10] - d[ 2]*d[ 4]*d[11] - d[ 3]*d[ 6]*d[ 8]) * invDet,
				(d[ 4]*d[ 9]*d[15] + d[ 5]*d[11]*d[12] + d[ 7]*d[ 8]*d[13] - d[ 4]*d[11]*d[13] - d[ 5]*d[ 8]*d[15] - d[ 7]*d[ 9]*d[12]) * invDet,
				(d[ 0]*d[11]*d[13] + d[ 1]*d[ 8]*d[15] + d[ 3]*d[ 9]*d[12] - d[ 0]*d[ 9]*d[15] - d[ 1]*d[11]*d[12] - d[ 3]*d[ 8]*d[13]) * invDet,
				(d[ 0]*d[ 5]*d[15] + d[ 1]*d[ 7]*d[12] + d[ 3]*d[ 4]*d[13] - d[ 0]*d[ 7]*d[13] - d[ 1]*d[ 4]*d[15] - d[ 3]*d[ 5]*d[12]) * invDet,
				(d[ 0]*d[ 7]*d[ 9] + d[ 1]*d[ 4]*d[11] + d[ 3]*d[ 5]*d[ 8] - d[ 0]*d[ 5]*d[11] - d[ 1]*d[ 7]*d[ 8] - d[ 3]*d[ 4]*d[ 9]) * invDet,
				(d[ 4]*d[10]*d[13] + d[ 5]*d[ 8]*d[14] + d[ 6]*d[ 9]*d[12] - d[ 4]*d[ 9]*d[14] - d[ 5]*d[10]*d[12] - d[ 6]*d[ 8]*d[13]) * invDet,
				(d[ 0]*d[ 9]*d[14] + d[ 1]*d[10]*d[12] + d[ 2]*d[ 8]*d[13] - d[ 0]*d[10]*d[13] - d[ 1]*d[ 8]*d[14] - d[ 2]*d[ 9]*d[12]) * invDet,
				(d[ 0]*d[ 6]*d[13] + d[ 1]*d[ 4]*d[14] + d[ 2]*d[ 5]*d[12] - d[ 0]*d[ 5]*d[14] - d[ 1]*d[ 6]*d[12] - d[ 2]*d[ 4]*d[13]) * invDet,
				(d[ 0]*d[ 5]*d[10] + d[ 1]*d[ 6]*d[ 8] + d[ 2]*d[ 4]*d[ 9] - d[ 0]*d[ 6]*d[ 9] - d[ 1]*d[ 4]*d[10] - d[ 2]*d[ 5]*d[ 8]) * invDet
		};
		System.arraycopy(dt, 0, d, 0, d.length);
		return this;
	}

	/**
	 * Returns the determinant of this matrix.
	 */
	public float determinant() {
		float det22_33 = d[10]*d[15] - d[14]*d[11];
		float det21_33 = d[ 9]*d[15] - d[13]*d[11];
		float det21_32 = d[ 9]*d[14] - d[13]*d[10];
		float det20_33 = d[ 8]*d[15] - d[12]*d[11];
		float det20_32 = d[ 8]*d[14] - d[12]*d[10];
		float det20_31 = d[ 8]*d[13] - d[12]*d[ 9];

		float det11_22_33 = d[5]*det22_33 - d[6]*det21_33 + d[7]*det21_32;
		float det10_22_33 = d[4]*det22_33 - d[6]*det20_33 + d[7]*det20_32;
		float det10_21_33 = d[4]*det21_33 - d[5]*det20_33 + d[7]*det20_31;
		float det10_21_32 = d[4]*det21_32 - d[5]*det20_32 + d[6]*det20_31;

		return d[0]*det11_22_33 - d[1]*det10_22_33 + d[2]*det10_21_33 - d[3]*det10_21_32;
	}

	/**
	 * Returns the trace of this matrix.
	 */
	public float trace() {
		return d[0] + d[5] + d[10] + d[15];
	}

	/**
	 * Returns the raw underlying mutable array.
	 */
	public float[] getArray() {
		return d;
	}
}
